/*
 *	surface_normal
 *
 *	v2: for use wit trumpet_geom files which have strongly curved surfaces (round_geom.c)
 *
 *	process the solid instrument geometry to find all the surface points and 
 *	compute the surface normal at each of these points
 *
 *	read the solid geometry from file geometry_instrument.dot produced by geom_3d_v4
 *
 *	there are two kinds of surface points:
 *		type A: which have one or more nearest neighbors (nn) open
 *		type B: which have one next nearest neighbors open - these are 
 *			concave ridge lines
 *		type C: which have one third nearest neighbors open - these are 
 *			concave corners 
 *
 *	usage: surface_normal_v2 geom_file 
 *		needed info is read from files geometry_instrument.dat, geometry_left_lip.dat
 *		and geometry_right_lip.dat 
 */

#include <stdio.h>
#include <math.h>
#include <strings.h>
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <omp.h>

#define OPEN		0
#define TYPE_A		1
#define TYPE_B		2
#define TYPE_C		3
#define INTERIOR	4
#define LEFT_LIP	5
#define RIGHT_LIP	6
#define LIP		7
#define NOT_LIP		8

#define N_S	4000000

#define N_X_MAX	2510
#define N_Y_MAX	410
#define N_Z_MAX	410

char point_type[N_X_MAX][N_Y_MAX][N_Z_MAX];

int n_surface;

int i_surface[N_S];
int j_surface[N_S];
int k_surface[N_S];
double i_hat[N_S];
double j_hat[N_S];
double k_hat[N_S];

int i_min,i_max,j_min,j_max,k_min,k_max;

int n_A,n_B,n_C;
double dx_min,dy_min,dz_min;
int trouble_count;

main(int argc,char *argv[])
{
	printf("reading info from geometry_instrument.dat, geometry_left_lip.dat");
	printf("and geometry_right_lip.dat\n");
	sleep(1);
	init_grid();
	readin();
// fprintf(stderr,"back from readin\n");
	find_surface_points_A();
// fprintf(stderr,"back from find surface points\n");
	find_surface_points_B();
	save_surface_normals();
}

init_grid()
{
	int i,j,k;

	for(i = 0; i < N_X_MAX; i++) {
		for(j = 0; j < N_Y_MAX; j++) {
			for(k = 0; k < N_Z_MAX; k++) {
				point_type[i][j][k] = OPEN;
			}
		}
	}

	return;
}

save_surface_normals()
{
	int i,j,k;
	FILE *fp_out;
	double norm;

// 	put info on suface points and their surface normals into file surface_point_info.dat
//	store as i,j,k coordinates followed by i_hat,j_hat,k_hat
//	note that i_hat, etc., are doubles (not ints)

	fp_out = fopen("surface_point_info.dat","w");

	for(i = 0; i < n_surface; i++) {
		fprintf(fp_out,"%d\t%d\t%d\t%g\t%g\t%g\n",i_surface[i],j_surface[i],
			k_surface[i],i_hat[i],j_hat[i],k_hat[i]);
// printf("%d %g %g %g\n",i,i_hat[i],j_hat[i],k_hat[i]);
	}
	fclose(fp_out);
	
	return;
}

find_surface_points_A()
{
	int i,j,k;
	double  x_sum,y_sum,z_sum;
	int n_open;
	double norm;

	int n_nearest_neighbors; // number of occupied nn

//	look for type A points
	n_A = 0;
	n_surface = 0;

	trouble_count = 0;

fprintf(stderr,"starting find surface points\n");
fprintf(stderr,"%d\t%d\t%d\t%d\t%d\t%d\n",i_min,i_max,j_min,j_max,k_min,k_max);

	for(i = i_min; i <= i_max; i++) {
		for(j = j_min; j <= j_max; j++) {
			for(k = k_min; k <= k_max; k++) {
			   if(point_type[i][j][k] == INTERIOR) {
				n_nearest_neighbors = 0; 
				x_sum = y_sum = z_sum = 0;
				n_open = 0;
				if((point_type[i+1][j][k] == OPEN) && (neighbors(i,j,k) != LIP)) { 
					x_sum += 1;
					++n_open;
				}
				if((point_type[i-1][j][k] == OPEN) && (neighbors(i,j,k) != LIP)) {
					x_sum -= 1;
					++n_open;
				}
				if((point_type[i][j+1][k] == OPEN) && (neighbors(i,j,k) != LIP)) {
					y_sum += 1;
					++n_open;
				}
				if((point_type[i][j-1][k] == OPEN) && (neighbors(i,j,k) != LIP)) {
					y_sum -= 1;
					++n_open;
				}
				if((point_type[i][j][k+1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					z_sum += 1;
					++n_open;
				}
				if((point_type[i][j][k-1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					z_sum -= 1;
					++n_open;
				}
		
				if(n_open > 0) {
					point_type[i][j][k] = TYPE_A;
					i_surface[n_surface] = i;
					j_surface[n_surface] = j;
					k_surface[n_surface] = k;
					i_hat[n_surface] = x_sum / n_open;
					j_hat[n_surface] = y_sum / n_open;
					k_hat[n_surface] = z_sum / n_open;
					norm = i_hat[n_surface]*i_hat[n_surface] 
						+ j_hat[n_surface]*j_hat[n_surface] 
						+ k_hat[n_surface]*k_hat[n_surface];
					norm = sqrt(norm);
					i_hat[n_surface] /= norm;
					j_hat[n_surface] /= norm;
					k_hat[n_surface] /= norm;
// printf("%d %d %d %g %g %g\n",i,j,k,i_hat[n_surface],j_hat[n_surface],k_hat[n_surface]);
					++n_surface;
					++n_A;
/*
if(fabs(norm) <= 1e-6) {
printf("trouble in type A at i/j/k = %d / %d / %d\n",i,j,k);
printf("\tcurrent point type = %d\ttnn point types = %d / %d\t%d / %d\t%d / %d\n",point_type[i][j][k],point_type[i-1][j][k],point_type[i+1][j][k],
point_type[i][j-1][k], point_type[i][j+1][k], point_type[i][j][k-1], point_type[i][j][k+1]); 
if(++trouble_count > 5) return;
}
*/
				}	
			   }
			}
		}
	}
	fprintf(stderr,"number of type A surface points = %d\n",n_A);
	fprintf(stderr,"number of surface points = %d\n",n_surface);

	return;
}

int readin()
{
	int i,j,k,n;
	FILE *fp_in;
	double x,delta;

	if((fp_in = fopen("geometry_instrument.dat","r")) == NULL) {
		fprintf(stderr,"can't open geometry_instrument.dat\n");
		(void)exit(0);
	}

	n = 0;
	if(fscanf(fp_in,"%d %d %d",&i,&j,&k) == EOF) {
		fprintf(stderr,"no data in geometry_instrument.dat\n");
		(void)exit(0);
	}
	i_min = i_max = i;
	j_min = j_max = j;
	k_min = k_max = k;
	n = 1;

	while(1) {
		if(fscanf(fp_in,"%d %d %d",&i,&j,&k) == EOF) break;
		if(i_min > i) i_min = i;
		if(i_max < i) i_max = i;

		if(j_min > j) j_min = j;
		if(j_max < j) j_max = j;

		if(k_min > k) k_min = k;
		if(k_max < k) k_max = k;
		++n;
	}
	fclose(fp_in);

fprintf(stderr,"n points = %d\n",n);

	fp_in = fopen("geometry_instrument.dat","r");

	while(1) {
		if(fscanf(fp_in,"%d %d %d",&i,&j,&k) == EOF) break;
		point_type[i][j][k] = INTERIOR;
	}
	fclose(fp_in);

	if((fp_in = fopen("geometry_left_lip.dat","r")) == NULL) {
		fprintf(stderr,"can't open geometry_left_lip.dat\n");
		(void)exit(0);
	}

	while(1) {
		if(fscanf(fp_in,"%d %d %d",&i,&j,&k) == EOF) break;
		point_type[i][j][k] = LEFT_LIP;
	}
	fclose(fp_in);

	if((fp_in = fopen("geometry_right_lip.dat","r")) == NULL) {
		fprintf(stderr,"can't open geometry_right_lip.dat\n");
		(void)exit(0);
	}

	while(1) {
		if(fscanf(fp_in,"%d %d %d",&i,&j,&k) == EOF) break;
		point_type[i][j][k] = RIGHT_LIP;
	}
	fclose(fp_in);

/*
// now read in grid spacings near instrument - assume the smallest ones are
// near the instrument surface

	if((fp_in = fopen("x_grid_spacing","r")) == NULL) {
		fprintf(stderr,"can't open x_grid_spacing\n");
		(void)exit(0);
	}

	fscanf(fp_in,"%lf %lf",&x,&dx_min);

	while(1) {
		if(fscanf(fp_in,"%lf %lf",&x,&delta) == EOF) break;
		if(dx_min > delta) dx_min = delta;
	}
	fclose(fp_in);

	if((fp_in = fopen("y_grid_spacing","r")) == NULL) {
		fprintf(stderr,"can't open y_grid_spacing\n");
		(void)exit(0);
	}

	fscanf(fp_in,"%lf %lf",&x,&dy_min);

	while(1) {
		if(fscanf(fp_in,"%lf %lf",&x,&delta) == EOF) break;
		if(dy_min > delta) dy_min = delta;
	}
	fclose(fp_in);

	if((fp_in = fopen("z_grid_spacing","r")) == NULL) {
		fprintf(stderr,"can't open z_grid_spacing\n");
		(void)exit(0);
	}

	fscanf(fp_in,"%lf %lf",&x,&dz_min);

	while(1) {
		if(fscanf(fp_in,"%lf %lf",&x,&delta) == EOF) break;
		if(dz_min > delta) dz_min = delta;
	}
	fclose(fp_in);

	printf("min dx / dy / dz = %g / %g / %g \n",dx_min,dy_min,dz_min);
*/

	return;
}
	
find_surface_points_B()
{
	int i,j,k;
	double  x_sum,y_sum,z_sum;
	int n_open;
	double norm;

	int n_next_nearest_neighbors; // number of occupied nn

//	look for type B points
	n_B = 0;

	trouble_count = 0;

fprintf(stderr,"starting find surface points B\n");

	for(i = i_min; i <= i_max; i++) {
		for(j = j_min; j <= j_max; j++) {
			for(k = k_min; k <= k_max; k++) {
			   if(point_type[i][j][k] == INTERIOR) {
				n_next_nearest_neighbors = 0; 
				x_sum = y_sum = z_sum = 0;
				n_open = 0;
				if((point_type[i+1][j+1][k] == OPEN) && (neighbors(i,j,k) != LIP)) { 
					x_sum += 1;
					y_sum += 1;
					++n_open;
				}
				if((point_type[i+1][j-1][k] == OPEN) && (neighbors(i,j,k) != LIP)) { 
					x_sum += 1;
					y_sum -= 1;
					++n_open;
				}
				if((point_type[i+1][j][k+1] == OPEN) && (neighbors(i,j,k) != LIP)) { 
					x_sum += 1;
					z_sum += 1;
					++n_open;
				}
				if((point_type[i+1][j][k-1] == OPEN) && (neighbors(i,j,k) != LIP)) { 
					x_sum += 1;
					z_sum -= 1;
					++n_open;
				}
				if((point_type[i-1][j+1][k] == OPEN) && (neighbors(i,j,k) != LIP)) {
					x_sum -= 1;
					y_sum += 1;
					++n_open;
				}
				if((point_type[i-1][j][k+1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					x_sum -= 1;
					z_sum += 1;
					++n_open;
				}
				if((point_type[i-1][j-1][k] == OPEN)  && (neighbors(i,j,k) != LIP)) {
					x_sum -= 1;
					y_sum -= 1;
					++n_open;
				}
				if((point_type[i-1][j][k-1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					x_sum -= 1;
					z_sum -= 1;
					++n_open;
				}
				if((point_type[i][j+1][k+1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					y_sum += 1;
					z_sum += 1;
					++n_open;
				}
				if((point_type[i][j+1][k-1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					y_sum += 1;
					z_sum -= 1;
					++n_open;
				}
				if((point_type[i][j-1][k+1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					y_sum -= 1;
					z_sum += 1;
					++n_open;
				}
				if((point_type[i][j-1][k-1] == OPEN) && (neighbors(i,j,k) != LIP)) {
					y_sum -= 1;
					z_sum -= 1;
					++n_open;
				}
		
				if(n_open > 0) {
					point_type[i][j][k] = TYPE_B;
					i_surface[n_surface] = i;
					j_surface[n_surface] = j;
					k_surface[n_surface] = k;
					i_hat[n_surface] = x_sum / n_open;
					j_hat[n_surface] = y_sum / n_open;
					k_hat[n_surface] = z_sum / n_open;
					norm = i_hat[n_surface]*i_hat[n_surface] 
						+ j_hat[n_surface]*j_hat[n_surface] 
						+ k_hat[n_surface]*k_hat[n_surface];
					norm = sqrt(norm);
					i_hat[n_surface] /= norm;
					j_hat[n_surface] /= norm;
					k_hat[n_surface] /= norm;
// printf("%d %d %d %g %g %g\n",i,j,k,i_hat[n_surface],j_hat[n_surface],k_hat[n_surface]);
					++n_surface;
					++n_B;
/*
if(norm <= 1e-6) {
printf("trouble in type B at i/j/k = %d / %d / %d\n",i,j,k);
printf("\tcurrent point type = %d\tnn point types = %d / %d\t%d / %d\t%d / %d\n",point_type[i][j][k],point_type[i-1][j][k],point_type[i+1][j][k],
point_type[i][j-1][k], point_type[i][j+1][k], point_type[i][j][k-1], point_type[i][j][k+1]); 
if(++trouble_count> 5) return;
}
*/
				}	
			   }
			}
		}
	}
	fprintf(stderr,"number of type B surface points = %d\n",n_B);
	fprintf(stderr,"total number of surface points = %d\n",n_surface);

	return;
}

/*
 * return LIP if any of the nn neighbors of i,j,k is a lip point
 * return NOT_LIP otherwise
 */
int
neighbors(int i,int j,int k)
{
	if(point_type[i+1][j][k] == RIGHT_LIP) return(LIP);
	if(point_type[i+1][j][k] == LEFT_LIP) return(LIP);

	if(point_type[i-1][j][k] == RIGHT_LIP) return(LIP);
	if(point_type[i-1][j][k] == LEFT_LIP) return(LIP);

	if(point_type[i][j+1][k] == RIGHT_LIP) return(LIP);
	if(point_type[i][j+1][k] == LEFT_LIP) return(LIP);

	if(point_type[i][j-1][k] == RIGHT_LIP) return(LIP);
	if(point_type[i][j-1][k] == LEFT_LIP) return(LIP);

	if(point_type[i][j][k+1] == RIGHT_LIP) return(LIP);
	if(point_type[i][j][k+1] == LEFT_LIP) return(LIP);

	if(point_type[i][j][k-1] == RIGHT_LIP) return(LIP);
	if(point_type[i][j][k-1] == LEFT_LIP) return(LIP);

	return(NOT_LIP);
}
